# -*- coding: utf-8 -*-

import numpy as np
import casadi
from functools import cmp_to_key
import copy
import const

def ACircle(p, e, sina):
    pe = np.linalg.norm(p - e)
    r = pe * sina / (1 - sina**2)
    oe = r / sina
    if pe <= 0.0:
        pe = 0.001
    o = e + oe / pe * (p - e)
    return o, r

def inv_ACircle(e, o, sina):
    oe = np.linalg.norm(o - e)
    pe = oe * (1 - sina**2)
    if oe <= 0.0:
        oe = 0.001
    p = e + pe / oe * (o - e)
    return p


def Rotate(p, o, a):
    vec = p - o
    cosa = np.cos(a)
    sina = np.sin(a)
    x = o[0] + cosa * vec[0] + sina * vec[1]
    y = o[1] - sina * vec[0] + cosa * vec[1]
    return x, y


class State(object):

    def __init__(self, n_purser=3):
        self.e_pos = np.zeros((2,))
        self.e_vmax = 1.0
        self.p_n = n_purser
        self.p_pos = np.zeros((self.p_n, 2))
        self.p_vmax = np.zeros((self.p_n,))
        self.p_vmax[:] = np.sin(np.pi / n_purser) * self.e_vmax


def SolveState1(state, pix):
    r = np.zeros((state.p_n, ))
    for i in range(state.p_n):
        r[i] = np.linalg.norm(state.e_pos - state.p_pos[i])
    def cmp_(a, b):
        return r[a] - r[b]
    ix = list(range(state.p_n))
    ix.sort(key=cmp_to_key(cmp_))
    ix = ix[: ix.index(pix)+1]
    r = np.zeros((len(ix), ))
    o = np.zeros((len(ix), 2))
    for i, j in enumerate(ix):
        o_, r_ = ACircle(state.p_pos[j], state.e_pos, state.p_vmax[j] / state.e_vmax)
        o[i, :] = o_ - state.e_pos
        r[i] = r_
    rv, av = Opti1(o[:-1], r[:-1], o[-1], r[-1])
    return av


def Opti1(o, r, o0, r0, a_init=0.0):
    vec  = o0.copy()
    r_init = np.linalg.norm(vec)
    opt = casadi.Opti()
    angle = opt.variable()
    r_var = opt.variable()
    # opt.minimize(r_var**2 * 100 + angle**2/100.0)
    opt.minimize(r_var**2)
    vec  /= np.linalg.norm(vec)
    cosa = np.cos(angle)
    sina = np.sin(angle)
    x = cosa * vec[0] + sina * vec[1]
    y = - sina * vec[0] + cosa * vec[1]
    x = x * r_var
    y = y * r_var
    for i in range(o.shape[0]):
        opt.subject_to(
            (o[i, 0] - x)**2 + (o[i, 1] - y)**2 >= (r[i] + r0)**2
        )
    opt.subject_to(angle <= np.pi)
    opt.subject_to(-np.pi <= angle)
    opt.subject_to(r_var >= r_init)
    opt.set_initial(angle, a_init)
    opt.set_initial(r_var, r_init)
    opt.solver('ipopt', const.OPTION)
    # try:
    #     sol = opt.solve()
    # except BaseException as e:
    #     return None, None
    sol = opt.solve()
    r_var = sol.value(r_var)
    angle = sol.value(angle)
    return r_var - r_init, angle


def Opti2(o, r, o0, r0, a_init=0.0):
    vec  = o0.copy()
    opt = casadi.Opti()
    angle = opt.variable()
    opt.minimize(angle**2)
    cosa = np.cos(angle)
    sina = np.sin(angle)
    x = cosa * vec[0] + sina * vec[1]
    y = - sina * vec[0] + cosa * vec[1]
    for i in range(o.shape[0]):
        opt.subject_to(
            (o[i, 0] - x)**2 + (o[i, 1] - y)**2 >= (r[i] + r0)**2
        )
    opt.subject_to(angle <= np.pi)
    opt.subject_to(-np.pi <= angle)
    opt.set_initial(angle, a_init)
    opt.solver('ipopt', const.OPTION)
    # try:
    #     sol = opt.solve()
    # except BaseException as e:
    #     return None
    sol = opt.solve()
    angle = sol.value(angle)
    return angle

def SolveState2(state, pix):
    t = np.zeros((state.p_n, ))
    for i in range(state.p_n):
        t[i] = np.linalg.norm(state.e_pos - state.p_pos[i]) / state.p_vmax[i]
    def cmp_(a, b):
        return t[a] - t[b]
    ix = list(range(state.p_n))
    ix.sort(key=cmp_to_key(cmp_))
    r = np.zeros((state.p_n, ))
    o = np.zeros((state.p_n, 2))
    for i in range(state.p_n):
        o_, r_ = ACircle(state.p_pos[i], state.e_pos, state.p_vmax[i] / state.e_vmax)
        o[i, :] = o_ - state.e_pos
        r[i] = r_
    if pix == ix[0]:
        return 0.0
    ix = ix[:1] + ix[ix.index(pix) + 1:]
    rlst = [0.0] * 3
    alst = [0.0] * 3
    for i, a_init in enumerate([0.0, np.pi, -np.pi]):
        rlst[i], alst[i] = Opti1(o[ix], r[ix], o[pix], r[pix], a_init=a_init)
    if  min(rlst)<= 0.0:
        av = Opti2(o[ix], r[ix], o[pix], r[pix])
    else:
        av = alst[rlst.index(min(rlst))]
    return av


def SolveState3(state):
    ix = GetOrder(state)
    ix.reverse()
    state2 = copy.deepcopy(state)
    angle_lst = [0.0] * state.p_n
    for i in ix[:-1]:
        angle = SolveState2(state2, i)
        angle_lst[i] = angle
        x, y = Rotate(state2.p_pos[i], state2.e_pos, angle_lst[i])
        state2.p_pos[i] = [x, y]
    return angle_lst


def GetOrder(state):
    t = np.zeros((state.p_n, ))
    for i in range(state.p_n):
        t[i] = np.linalg.norm(state.e_pos - state.p_pos[i]) / state.p_vmax[i]
    def cmp_(a, b):
        return t[a] - t[b]
    ix = list(range(state.p_n))
    ix.sort(key=cmp_to_key(cmp_))
    return ix